#include "PointParticle.h"

PointParticle::PointParticle() {
}

PointParticle::PointParticle(const Vector3d& pos) {
    this->pos = pos;
    this->vel = Vector3d(0, 0, 0);
    this->acc = Vector3d(0, 0, 0);
}

PointParticle::PointParticle(const Vector3d& pos, const Vector3d& vel) {
    this->pos = pos;
    this->vel = vel;
    this->acc = Vector3d(0, 0, 0);
}

void PointParticle::eulerUpdate(double dt) {
    //pos += (vel * dt) + (0.5 * acc * dt * dt);
    vel += (acc * dt);
    pos += vel * dt;
    acc = Vector3d(0, 0, 0);
}

void PointParticle::velocityVerletUpdate(double dt) {
    vel = vel + 0.5 * acc * dt;
    pos = pos + vel * dt + 0.5 * acc * dt * dt;
    vel = vel + 0.5 * acc * dt;
    acc = Vector3d(0, 0, 0);
}

Vector3d PointParticle::hookesLawForce(double k, double zeta, double restDist, PointParticle& other) const {
    Vector3d dir = other.pos - pos;
    Vector3d relVel = other.vel - vel;
    Vector3d velDir = vel.getNormalized();
    double dist = dir.normalize();
    
    Vector3d spring = k * dir * (dist - restDist);
    Vector3d damping = -zeta * (dir * relVel) * dir;

    return spring + damping;
}

void PointParticle::hookesLaw(double k, double zeta, double restDist, PointParticle& other) {
    Vector3d force = hookesLawForce(k, zeta, restDist, other);

    acc += force;
    other.acc -= force;
}
